#include "inc/hw_types.h"		// tBoolean
#include "inc/hw_memmap.h"
#include "RASLib/init.h"
#include "driverlib/gpio.h"
#include "driverlib/sysctl.h"

#include "RASLib/motor.h"
#include "RASLib/encoder.h"
#include "RASLib/servo.h"
 
void initMotors(void);
void initLineSensor(void);
void initUART(void);
void initIRSensor(void);
void Wait(long ms);
void initServo(void);

/**
 *This uses PID to follow a wall and stay straight
 */

void wallFollowTemplate(void){
	int speed=127;
	int waitTime=5000;
	long distance;//This is the distance from the wall the robot tries to maintain

	//PID values   DO NOT CHANGE!!! Unless you want to
	float Kp=1.45;//Keep this
	float Ki=.00001;
	float Kd=20;
	int currentError=0,lastError=0;
	int intError=0;
	int integral=0;
	int derivative=0;
	//Random variables 
    int Pout;
	int Iout;
	int Dout;
	float MV;
	float absMV;
	signed char velocity;
	unsigned char servoPosition=255;

	LockoutProtection();
	InitializeMCU();
	initIRSensor();
	initMotors();
	initLineSensor();
	initUART();
	initServo();
	SetDischargeTime(220);
						  
	SetServoPosition(SERVO_0, servoPosition);//This is all the way to the right
	Wait(2000);			   
	SetMotorPowers(speed,speed);
	//Wait(waitTime);
	//SetMotorPowers(0,0);
	intError=getADCValue();
	intError/=20;//divides by 20 to get rid of any unnecessary error
	distance=intError;

	while(1){
		lastError=currentError;
		//Now it's time to compute the error...
		intError=distance-getADCValue()/20;
		currentError=intError; 
		//Now that we have the error, we can update the derivative and integral
		derivative=currentError-lastError;
		integral+=currentError;
		//It is now time to compute Pout, Iout, and Dout, thus getting MV (the manipulated variable)
		//It should be noted that a positive derivative means the robot needs to turn left, and a
		//negative derivative means the robot needs to turn right. A positive integral means the
		//robot needs to skew left, and a negative integral value means the robot needs to skew right
		Pout=Kp*currentError;
		Iout=Ki*integral;
		Dout=Kd*derivative;
		MV=Pout+Iout+Dout;
		//If MV is positive, the robot needs to go left. If MV is negative, the robot needs to go right
		//But... we first need to come up with a linear relationship relating MV to the speed of the
		//reduced wheel (This means an mx+b relationship... more like a b-mx relationship)
		absMV=MV;
		if(MV<0)absMV*=-1;
		velocity=(signed char)(speed-absMV);
		if(velocity<-128)velocity=-128;
		if(MV>0){
			SetMotorPowers(speed,velocity);
		}
		else if(MV<0){
			SetMotorPowers(velocity,speed);
		}
		else{//MV equals 0; never going to happen, but might?
			SetMotorPowers(speed,speed);
		}
	}
}
